

#include "gps.h"
#include "config.h"

#include "Arduino.h"

#include <avr/delay.h>
#include <avr/pgmspace.h>
#include <EEPROM.h>

#include <math.h>





 // Buffer for GPS string
 char GPSbuffer[100]={0};


//==================================
// Decleration for GPS-variables
//==================================

// Has homeposition been set?
char homepos=0;
// Simple way to make a little delay before the homeposition is set. (It waits for GPS-fix, waits a couple of ekstra seconds and set homeposition)
int homeposcount=0;

// GPSfix - when '0' no satellite fix.
char GPSfix='0';

// Used to store all the GPS positions etc...
long lat1=0;
long lat2=0;
long lats=0;

long lon1=0;
long lon2=0;
long lons=0;

// Used to calculate line of sight.
long losy=0;
long losx=0;

// Variables to store home position:
long lathome = 0;
long lonhome = 0;
long los = 0;

// Direction home:
int homehead=0;
int homehead_r[3]={96,96,96};
int headi=0;
int arrow=0;
int arrowd;
unsigned char satellites[3]={3,3,3};
unsigned char satellitesr[3]={3,3,3};

unsigned char latitude_dir =0;
unsigned char longitude_dir =0;

float longitude_factor =1;
int lat_deg = 90;

  
unsigned char show_mah_km = EEPROM.read(1);
unsigned char show_decimals = EEPROM.read(2);
unsigned char altitude_offset_on = EEPROM.read(3);
unsigned char align_text = EEPROM.read(4);
unsigned char show_plane_pos = EEPROM.read(5);

int alt_alarm = EEPROM.read(10)+(EEPROM.read(11)<<8);
int los_alarm = EEPROM.read(12)+(EEPROM.read(13)<<8);
int volt_alarm =EEPROM.read(14)+(EEPROM.read(15)<<8);
int mah_alarm = EEPROM.read(16)+(EEPROM.read(17)<<8);



//========================================
// Data read from GPS
//========================================
 unsigned char time[14]={1,1,1,1,1,1,1,1,1,1,1,1};;
 unsigned char speedkn[10]={1,1,1,1,1};
 unsigned char speedkn2[10]={1,1,1,1,1};
 unsigned char speedkmw[5]={1,1,1,1,1};
 unsigned int speedkm =100;
 unsigned int speedkm2 =0;
 int latitude[15]={0};
 int longitude[15]={0};
 int head[10]={0};
 unsigned char altitude[10]={1,1,1,1,1,1};
 unsigned char altitude2[10]={1,1,1,1,1,1}; 
 unsigned char altituder[10]={1,1,1,1,1,1,1,1};
 unsigned char altituder2[10]={1,1,1,1,1,1,1,1};
 long altitude_offset =0;
 int altitude_negative=0;


 
 
 
//========================================
// For flight summary
//========================================
long max_los =0;
unsigned char max_losr[] = {3,3,3,3};

int max_speed =0;
unsigned char max_speedr[] = {3,3,3,3};

long kmh_total =0;
unsigned char total_distancer[] = {3,3,3,3,3};

long max_alt=0;
unsigned char max_altr[] = {3,3,3,3,3,3};

unsigned char landed =1;
long altitude_num=0;
long altitude_num2=0;
int altitude_int=0;

unsigned long flight_time =0;
unsigned char last_time=0;
unsigned char flight_timer[] = {3,3,3,3,3,3};

int avg_speed =0;
unsigned char avg_speedr[] = {3,3,3,3,3,3};

unsigned char loadbar[]={29,27,27,27,27,27,27,27,27,28};

unsigned char test=0;


unsigned char arrowr[] ={3,3,3};
int current_num=0;

unsigned int mahkm =0;
unsigned char mahkmr[]={3,3,3,3,3,3};  

unsigned int bufSpeed[6]={14, 14, 14, 14, 14, 14};
unsigned int bufLos[6]={14, 14, 14, 14, 14, 14};
unsigned int bufAlt[6]={14, 14, 14, 14, 14, 14};

unsigned int bufCurrent[6]={14, 14, 14, 14, 14, 14};
unsigned int bufVoltage[5]={12,12,12,12,12};
unsigned int bufMah[4]={12,12,12,12};

unsigned int bufTimer[5] = {12, 12, 12, 12, 12};
unsigned int bufRSSI[5] = {12, 12, 12, 12, 12};




//nChars counts dot as well so 123.4 is 5 characters
void fillBuffer(int num, unsigned int* buffer, int nChars, bool bDot = true)
{
    //int power = pow(10, nChars-1); //causes problems for some insane reason
    int power = 1;
    for (int j=0; j < nChars-1; j++)
        power *= 10;  
          
    for (int i = 0; i <= nChars; i++)
	{
	    if (bDot && (i == (nChars-2)))
	        buffer[i] = 1 << 5;
        else
        {
    	    int digit = num % power / (power / 10);
            buffer[i] = (digit + 3) << 5;
    		power /= 10;
	    }
	}
}

void gps() 
{
    int currentVal = 1458;
    int voltageVal = 118;
    int mahVal = 24;
    speedkm = 1235; //123.5kmh
    los = 341;
    altitude_num = 4560;
     
    fillBuffer(currentVal, bufCurrent, 5);
    fillBuffer(voltageVal, bufVoltage, 4);
    fillBuffer(mahVal, bufMah, 3);    	        
    
    
    fillBuffer(speedkm, bufSpeed, 5);    	        
    fillBuffer(los, bufLos, 5, false);    	        
    fillBuffer(altitude_num, bufAlt, 5, false);  	        
    
    //fillBuffer(altitude_num, bufTimer, 5, false);
    fillBuffer(altitude_num, bufRSSI, 5);
    
    satellitesr[0]=4; //this means 12 mofo
    satellitesr[1]=5;   
    
    //update flight timer
    flight_time = millis() / 1000;
     
    bufTimer[0] = ((flight_time/600)+3) << 5;
    bufTimer[1] = (((flight_time/60)%10)+3) << 5;
    bufTimer[2] = 2 << 5;
    bufTimer[3] = ((flight_time%60)/10+3) << 5;
    bufTimer[4] = ((flight_time%60)%10+3) << 5; 
}
